using System;
using System.Collections.Generic;
using System.Data.Linq;
using System.Text;

namespace GraphSpace
{


	public class BiSearch<PQ1, PQ2> where PQ1 : IPQ, new() where PQ2 : IPQ, new()
	{
		private PQ1 PriorityStuct1 = new PQ1 ();
		private PQ2 PriorityStuct2 = new PQ2 ();
		private int Mittelpunkt;
		//private double AbstandST;

		public int getDeleteMin ()
		{
			return PriorityStuct1.getDeleteMinCounter () + PriorityStuct2.getDeleteMinCounter ();
		}

		public BiSearch (int C)
		{
			PriorityStuct1.init (C);
			PriorityStuct2.init (C);
		}

		public BiSearch ()
		{
			
		}

		public double DoSearch (Graph G1, Graph G2, int initialKnoten, int endKnoten, out Knoten[] Path, bool GoalDirected1, bool GoalDirected2)
		{
			//AbstandST = Math.Abs (G1.V[initialKnoten].x - G1.V[endKnoten].x) + Math.Abs (G1.V[initialKnoten].y - G1.V[endKnoten].y);
			G1.V[initialKnoten].distance = 0;
			G1.V[initialKnoten].realdist = 0;
			G1.V[initialKnoten].foundBySearch = true;
			G1.V[initialKnoten].besucht = true;
			PriorityStuct1.insert (G1.V[initialKnoten]);
			
			G2.V[endKnoten].distance = 0;
			G2.V[endKnoten].realdist = 0;
			G2.V[endKnoten].foundBySearch = true;
			G2.V[endKnoten].besucht = true;
			PriorityStuct2.insert (G2.V[endKnoten]);
			
			while (true)
			{
				if (PriorityStuct1.getMinDistance () < PriorityStuct2.getMinDistance ())
				{
					if (DoSearchStep1 (G1, G2, G1.V[initialKnoten], G1.V[endKnoten], GoalDirected1))
						break;
				}

				else
				{
					// muss so sein!!!!!!!!!!!
					if (DoSearchStep2 (G1, G2, G2.V[endKnoten], G2.V[initialKnoten], GoalDirected2))
						break;
				}
			}
			
			List<Knoten> pathlist = new List<Knoten> ();
			Knoten pred = G1.V[Mittelpunkt];
			while (pred != null)
			{
				pathlist.Add (pred);
				pred = pred.predeseccor;
			}
			pathlist.Reverse ();
			pathlist.RemoveAt (pathlist.Count - 1);
			pred = G2.V[Mittelpunkt];
			if (pred != null)
				while (pred != null)
				{
					pathlist.Add (pred);
					pred = pred.predeseccor;
				}
			
			Path = pathlist.ToArray ();
			
			if (GoalDirected1 && GoalDirected2)
				return G1.V[Mittelpunkt].realdist + G2.V[Mittelpunkt].realdist;
			if (GoalDirected1 && !GoalDirected2)
				return G1.V[Mittelpunkt].realdist + G2.V[Mittelpunkt].distance;
			if (!GoalDirected1 && GoalDirected2)
				return G1.V[Mittelpunkt].distance + G2.V[Mittelpunkt].realdist;
			return G1.V[Mittelpunkt].distance + G2.V[Mittelpunkt].distance;
		}

		bool DoSearchStep1 (Graph G, Graph G2, Knoten initialKnoten, Knoten endKnoten, bool GoalDirected)
		{
			
			Knoten aktKnoten = PriorityStuct1.extractHead ();
			aktKnoten.foundBySearch = true;
			aktKnoten.besucht = true;
			if (G2.V[aktKnoten.id].foundBySearch)
			{
				Mittelpunkt = aktKnoten.id;
				return true;
			}
			
			//if (aktKnoten.Id == endKnoten.Id)
			//    break;
			
			for (int i = 0; i < G.A[aktKnoten.id].Count; i++)
			{
				Knoten ZielKnoten = (Knoten)G.A[aktKnoten.id][i].Vnach;
				//Unterschied zwischen normal oder zielgerichtet
				if (GoalDirected)
				{
					int AbstandX = Math.Abs (endKnoten.x - ZielKnoten.x);
					int AbstandY = Math.Abs (endKnoten.y - ZielKnoten.y);
					double d = aktKnoten.realdist + G.A[aktKnoten.id][i].Gewicht;
					double d_strich = d + AbstandX + AbstandY;
					//	Console.WriteLine("Zielknoten "+ ZielKnoten.id + " akt "+ZielKnoten.distance + " neu "+NewDist);
					if (!ZielKnoten.entfernt && d < ZielKnoten.realdist)
					{
						ZielKnoten.predeseccor = aktKnoten;
						ZielKnoten.realdist = d;
						
						if (!ZielKnoten.besucht)
						{
							//	Console.WriteLine(ZielKnoten.id + " neu eingefügt");
							ZielKnoten.distance = d_strich;
							ZielKnoten.besucht = true;
							PriorityStuct1.insert (ZielKnoten);
						}

						else
						{
							//			Console.WriteLine(ZielKnoten.id + " verbessert");
							PriorityStuct1.decreaseKey (ZielKnoten, d_strich);
						}
					}
				}

				else
				{
					double NewDist = aktKnoten.distance + (int)G.A[aktKnoten.id][i].Gewicht;
					//Console.WriteLine("Zielknoten "+ ZielKnoten.id + " akt "+ZielKnoten.distance + " neu "+NewDist);
					if (NewDist < G.A[aktKnoten.id][i].Vnach.distance)
					{
						ZielKnoten.predeseccor = aktKnoten;
						ZielKnoten.distance = NewDist;
						if (!ZielKnoten.besucht)
						{
							//		Console.WriteLine(ZielKnoten.id + " neu eingefügt");
							ZielKnoten.besucht = true;
							PriorityStuct1.insert (ZielKnoten);
						}

						else
						{
							//Console.WriteLine(ZielKnoten.id + " verbessert");
							PriorityStuct1.decreaseKey (ZielKnoten, NewDist);
						}
					}
				}
			}
			aktKnoten.entfernt = true;
			return false;
		}

		bool DoSearchStep2 (Graph G, Graph G2, Knoten initialKnoten, Knoten endKnoten, bool GoalDirected)
		{
			
			Knoten aktKnoten = PriorityStuct2.extractHead ();
			aktKnoten.foundBySearch = true;
			aktKnoten.besucht = true;
			if (G.V[aktKnoten.id].foundBySearch)
			{
				Mittelpunkt = aktKnoten.id;
				return true;
			}
			
			for (int i = 0; i < G2.Ar[aktKnoten.id].Count; i++)
			{
				Knoten ZielKnoten = (Knoten)G2.Ar[aktKnoten.id][i].Vnach;
				//Unterschied zwischen normal oder zielgerichtet
				if (GoalDirected)
				{
					int AbstandX = Math.Abs (endKnoten.x - ZielKnoten.x);
					int AbstandY = Math.Abs (endKnoten.y - ZielKnoten.y);
					double d = aktKnoten.realdist + (int)G2.Ar[aktKnoten.id][i].Gewicht;
					double d_strich = d + AbstandX + AbstandY;
					//	Console.WriteLine("Zielknoten "+ ZielKnoten.id + " akt "+ZielKnoten.distance + " neu "+NewDist);
					if (!ZielKnoten.entfernt && d < ZielKnoten.realdist)
					{
						ZielKnoten.predeseccor = aktKnoten;
						ZielKnoten.realdist = d;
						
						if (!ZielKnoten.besucht)
						{
							//	Console.WriteLine(ZielKnoten.id + " neu eingefügt");
							ZielKnoten.distance = d_strich;
							ZielKnoten.besucht = true;
							PriorityStuct2.insert (ZielKnoten);
						}

						else
						{
							//			Console.WriteLine(ZielKnoten.id + " verbessert");
							PriorityStuct2.decreaseKey (ZielKnoten, d_strich);
						}
					}
				}

				else
				{
					double NewDist = aktKnoten.distance + (int)G2.Ar[aktKnoten.id][i].Gewicht;
					//Console.WriteLine("Zielknoten "+ ZielKnoten.id + " akt "+ZielKnoten.distance + " neu "+NewDist);
					if (NewDist < G2.Ar[aktKnoten.id][i].Vnach.distance)
					{
						ZielKnoten.predeseccor = aktKnoten;
						ZielKnoten.distance = NewDist;
						if (!ZielKnoten.besucht)
						{
							//		Console.WriteLine(ZielKnoten.id + " neu eingefügt");
							ZielKnoten.besucht = true;
							PriorityStuct2.insert (ZielKnoten);
						}

						else
						{
							//Console.WriteLine(ZielKnoten.id + " verbessert");
							PriorityStuct2.decreaseKey (ZielKnoten, NewDist);
						}
					}
				}
			}
			aktKnoten.entfernt = true;
			return false;
		}
	}
}
